﻿using System;
using System.Collections.Generic;
using System.Linq;
using YJKRevitBase.Algorithm.Octree.Data;

namespace YJKRevitBase.Algorithm.Octree
{
	public class PointOctree<T> where T : class
	{
		public int Count { get; private set; }

		public BoundingBox MaxBounds
		{
			get
			{
				return new BoundingBox(this._rootNode.Center, new Point(this._rootNode.SideLength, this._rootNode.SideLength, this._rootNode.SideLength));
			}
		}

		public PointOctree(float initialWorldSize, Point initialWorldPos, float minNodeSize)
		{
			if (minNodeSize > initialWorldSize)
			{
				minNodeSize = initialWorldSize;
			}
			this.Count = 0;
			this._initialSize = initialWorldSize;
			this._minSize = minNodeSize;
			this._rootNode = new PointOctree<T>.Node(this._initialSize, this._minSize, initialWorldPos);
		}

		public void Add(T obj, Point objPos)
		{
			int num = 0;
			while (!this._rootNode.Add(obj, objPos))
			{
				this.Grow(objPos - this._rootNode.Center);
				if (++num > 20)
				{
					return;
				}
			}
			int count = this.Count;
			this.Count = count + 1;
		}

		public bool Remove(T obj)
		{
			bool flag = this._rootNode.Remove(obj);
			if (flag)
			{
				int count = this.Count;
				this.Count = count - 1;
				this.Shrink();
			}
			return flag;
		}

		public bool Remove(T obj, Point objPos)
		{
			bool flag = this._rootNode.Remove(obj, objPos);
			if (flag)
			{
				int count = this.Count;
				this.Count = count - 1;
				this.Shrink();
			}
			return flag;
		}

		public T[] GetNearby(Ray ray, float maxDistance)
		{
			List<T> list = new List<T>();
			this._rootNode.GetNearby(ref ray, ref maxDistance, list);
			return list.ToArray();
		}

		public T[] GetNearby(Point position, float maxDistance)
		{
			List<T> list = new List<T>();
			this._rootNode.GetNearby(ref position, ref maxDistance, list);
			return list.ToArray();
		}

		public ICollection<T> GetAll()
		{
			List<T> result = new List<T>(this.Count);
			this._rootNode.GetAll(result);
			return result;
		}

		private void Grow(Point direction)
		{
			int num = (direction.X >= 0f) ? 1 : -1;
			int num2 = (direction.Y >= 0f) ? 1 : -1;
			int num3 = (direction.Z >= 0f) ? 1 : -1;
			PointOctree<T>.Node rootNode = this._rootNode;
			float num4 = this._rootNode.SideLength / 2f;
			float baseLengthVal = this._rootNode.SideLength * 2f;
			Point point = this._rootNode.Center + new Point((float)num * num4, (float)num2 * num4, (float)num3 * num4);
			this._rootNode = new PointOctree<T>.Node(baseLengthVal, this._minSize, point);
			int rootPosIndex = PointOctree<T>.GetRootPosIndex(num, num2, num3);
			PointOctree<T>.Node[] array = new PointOctree<T>.Node[8];
			for (int i = 0; i < 8; i++)
			{
				if (i == rootPosIndex)
				{
					array[i] = rootNode;
				}
				else
				{
					num = ((i % 2 == 0) ? -1 : 1);
					num2 = ((i > 3) ? -1 : 1);
					num3 = ((i < 2 || (i > 3 && i < 6)) ? -1 : 1);
					array[i] = new PointOctree<T>.Node(this._rootNode.SideLength, this._minSize, point + new Point((float)num * num4, (float)num2 * num4, (float)num3 * num4));
				}
			}
			this._rootNode.SetChildren(array);
		}

		private void Shrink()
		{
			this._rootNode = this._rootNode.ShrinkIfPossible(this._initialSize);
		}

		private static int GetRootPosIndex(int xDir, int yDir, int zDir)
		{
			int num = (xDir > 0) ? 1 : 0;
			if (yDir < 0)
			{
				num += 4;
			}
			if (zDir > 0)
			{
				num += 2;
			}
			return num;
		}

		private PointOctree<T>.Node _rootNode;

		private readonly float _initialSize;

		private readonly float _minSize;

		private class Node
		{
			public Point Center { get; private set; }

			public float SideLength { get; private set; }

			public Node(float baseLengthVal, float minSizeVal, Point centerVal)
			{
				this.SetValues(baseLengthVal, minSizeVal, centerVal);
			}

			public bool Add(T obj, Point objPos)
			{
				if (!PointOctree<T>.Node.Encapsulates(this._bounds, objPos))
				{
					return false;
				}
				this.SubAdd(obj, objPos);
				return true;
			}

			public bool Remove(T obj)
			{
				bool flag = false;
				for (int i = 0; i < this._objects.Count; i++)
				{
					if (this._objects[i].Obj.Equals(obj))
					{
						flag = this._objects.Remove(this._objects[i]);
						break;
					}
				}
				if (!flag && this._children != null)
				{
					for (int j = 0; j < 8; j++)
					{
						flag = this._children[j].Remove(obj);
						if (flag)
						{
							break;
						}
					}
				}
				if (flag && this._children != null && this.ShouldMerge())
				{
					this.Merge();
				}
				return flag;
			}

			public bool Remove(T obj, Point objPos)
			{
				return PointOctree<T>.Node.Encapsulates(this._bounds, objPos) && this.SubRemove(obj, objPos);
			}

			public void GetNearby(ref Ray ray, ref float maxDistance, List<T> result)
			{
				this._bounds.Expand(new Point(maxDistance * 2f, maxDistance * 2f, maxDistance * 2f));
				bool flag = this._bounds.IntersectRay(ray);
				this._bounds.Size = this._actualBoundsSize;
				if (!flag)
				{
					return;
				}
				for (int i = 0; i < this._objects.Count; i++)
				{
					if (PointOctree<T>.Node.SqrDistanceToRay(ray, this._objects[i].Pos) <= maxDistance * maxDistance)
					{
						result.Add(this._objects[i].Obj);
					}
				}
				if (this._children != null)
				{
					for (int j = 0; j < 8; j++)
					{
						this._children[j].GetNearby(ref ray, ref maxDistance, result);
					}
				}
			}

			public void GetNearby(ref Point position, ref float maxDistance, List<T> result)
			{
				this._bounds.Expand(new Point(maxDistance * 2f, maxDistance * 2f, maxDistance * 2f));
				bool flag = this._bounds.Contains(position);
				this._bounds.Size = this._actualBoundsSize;
				if (!flag)
				{
					return;
				}
				for (int i = 0; i < this._objects.Count; i++)
				{
					if (Point.Distance(position, this._objects[i].Pos) <= maxDistance)
					{
						result.Add(this._objects[i].Obj);
					}
				}
				if (this._children != null)
				{
					for (int j = 0; j < 8; j++)
					{
						this._children[j].GetNearby(ref position, ref maxDistance, result);
					}
				}
			}

			public void GetAll(List<T> result)
			{
				result.AddRange(from o in this._objects
				select o.Obj);
				if (this._children != null)
				{
					for (int i = 0; i < 8; i++)
					{
						this._children[i].GetAll(result);
					}
				}
			}

			public void SetChildren(PointOctree<T>.Node[] childOctrees)
			{
				if (childOctrees.Length != 8)
				{
					return;
				}
				this._children = childOctrees;
			}

			public PointOctree<T>.Node ShrinkIfPossible(float minLength)
			{
				if (this.SideLength < 2f * minLength)
				{
					return this;
				}
				if (this._objects.Count == 0 && this._children.Length == 0)
				{
					return this;
				}
				int num = -1;
				for (int i = 0; i < this._objects.Count; i++)
				{
					PointOctree<T>.Node.OctreeObject octreeObject = this._objects[i];
					int num2 = this.BestFitChild(octreeObject.Pos);
					if (i != 0 && num2 != num)
					{
						return this;
					}
					if (num < 0)
					{
						num = num2;
					}
				}
				if (this._children != null)
				{
					bool flag = false;
					for (int j = 0; j < this._children.Length; j++)
					{
						if (this._children[j].HasAnyObjects())
						{
							if (flag)
							{
								return this;
							}
							if (num >= 0 && num != j)
							{
								return this;
							}
							flag = true;
							num = j;
						}
					}
				}
				if (this._children == null)
				{
					this.SetValues(this.SideLength / 2f, this._minSize, this._childBounds[num].Center);
					return this;
				}
				return this._children[num];
			}

			public static float SqrDistanceToRay(Ray ray, Point point)
			{
				return Point.Cross(ray.Direction, point - ray.Origin).SqrMagnitude;
			}

			private void SetValues(float baseLengthVal, float minSizeVal, Point centerVal)
			{
				this.SideLength = baseLengthVal;
				this._minSize = minSizeVal;
				this.Center = centerVal;
				this._actualBoundsSize = new Point(this.SideLength, this.SideLength, this.SideLength);
				this._bounds = new BoundingBox(this.Center, this._actualBoundsSize);
				float num = this.SideLength / 4f;
				float num2 = this.SideLength / 2f;
				Point size = new Point(num2, num2, num2);
				this._childBounds = new BoundingBox[8];
				this._childBounds[0] = new BoundingBox(this.Center + new Point(-num, num, -num), size);
				this._childBounds[1] = new BoundingBox(this.Center + new Point(num, num, -num), size);
				this._childBounds[2] = new BoundingBox(this.Center + new Point(-num, num, num), size);
				this._childBounds[3] = new BoundingBox(this.Center + new Point(num, num, num), size);
				this._childBounds[4] = new BoundingBox(this.Center + new Point(-num, -num, -num), size);
				this._childBounds[5] = new BoundingBox(this.Center + new Point(num, -num, -num), size);
				this._childBounds[6] = new BoundingBox(this.Center + new Point(-num, -num, num), size);
				this._childBounds[7] = new BoundingBox(this.Center + new Point(num, -num, num), size);
			}

			private void SubAdd(T obj, Point objPos)
			{
				if (this._objects.Count < 8 || this.SideLength / 2f < this._minSize)
				{
					PointOctree<T>.Node.OctreeObject item = new PointOctree<T>.Node.OctreeObject
					{
						Obj = obj,
						Pos = objPos
					};
					this._objects.Add(item);
					return;
				}
				int num;
				if (this._children == null)
				{
					this.Split();
					if (this._children == null)
					{
						return;
					}
					for (int i = this._objects.Count - 1; i >= 0; i--)
					{
						PointOctree<T>.Node.OctreeObject octreeObject = this._objects[i];
						num = this.BestFitChild(octreeObject.Pos);
						this._children[num].SubAdd(octreeObject.Obj, octreeObject.Pos);
						this._objects.Remove(octreeObject);
					}
				}
				num = this.BestFitChild(objPos);
				this._children[num].SubAdd(obj, objPos);
			}

			private bool SubRemove(T obj, Point objPos)
			{
				bool flag = false;
				for (int i = 0; i < this._objects.Count; i++)
				{
					if (this._objects[i].Obj.Equals(obj))
					{
						flag = this._objects.Remove(this._objects[i]);
						break;
					}
				}
				if (!flag && this._children != null)
				{
					int num = this.BestFitChild(objPos);
					flag = this._children[num].SubRemove(obj, objPos);
				}
				if (flag && this._children != null && this.ShouldMerge())
				{
					this.Merge();
				}
				return flag;
			}

			private void Split()
			{
				float num = this.SideLength / 4f;
				float baseLengthVal = this.SideLength / 2f;
				this._children = new PointOctree<T>.Node[8];
				this._children[0] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(-num, num, -num));
				this._children[1] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(num, num, -num));
				this._children[2] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(-num, num, num));
				this._children[3] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(num, num, num));
				this._children[4] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(-num, -num, -num));
				this._children[5] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(num, -num, -num));
				this._children[6] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(-num, -num, num));
				this._children[7] = new PointOctree<T>.Node(baseLengthVal, this._minSize, this.Center + new Point(num, -num, num));
			}

			private void Merge()
			{
				for (int i = 0; i < 8; i++)
				{
					PointOctree<T>.Node node = this._children[i];
					for (int j = node._objects.Count - 1; j >= 0; j--)
					{
						PointOctree<T>.Node.OctreeObject item = node._objects[j];
						this._objects.Add(item);
					}
				}
				this._children = null;
			}

			private static bool Encapsulates(BoundingBox outerBounds, Point point)
			{
				return outerBounds.Contains(point);
			}

			private int BestFitChild(Point objPos)
			{
				return ((objPos.X <= this.Center.X) ? 0 : 1) + ((objPos.Y >= this.Center.Y) ? 0 : 4) + ((objPos.Z <= this.Center.Z) ? 0 : 2);
			}

			private bool ShouldMerge()
			{
				int num = this._objects.Count;
				if (this._children != null)
				{
					foreach (PointOctree<T>.Node node in this._children)
					{
						if (node._children != null)
						{
							return false;
						}
						num += node._objects.Count;
					}
				}
				return num <= 8;
			}

			private bool HasAnyObjects()
			{
				if (this._objects.Count > 0)
				{
					return true;
				}
				if (this._children != null)
				{
					for (int i = 0; i < 8; i++)
					{
						if (this._children[i].HasAnyObjects())
						{
							return true;
						}
					}
				}
				return false;
			}

			private float _minSize;

			private BoundingBox _bounds;

			private readonly List<PointOctree<T>.Node.OctreeObject> _objects = new List<PointOctree<T>.Node.OctreeObject>();

			private PointOctree<T>.Node[] _children;

			private BoundingBox[] _childBounds;

			private const int NumObjectsAllowed = 8;

			private Point _actualBoundsSize;

			private class OctreeObject
			{
				public T Obj;

				public Point Pos;
			}
		}
	}
}
